
#include "pathOptimiz.h"

using namespace Eigen;

VectorXd timeAllocation( MatrixXd Path)
{ 
    VectorXd time(Path.rows() - 1);
    
    for(int i=0;i<(int)time.size();i++)
    {
        time(i)=(Path.row(i)-Path.row(i+1)).norm();
    }
    return time;
}

int Factorial(int x)
{
    int fac = 1;
    for(int i = x; i > 0; i--)
        fac = fac * i;
    return fac;
}


Eigen::MatrixXd PolyQPGeneration(const int d_order,const Eigen::MatrixXd &Path,const Eigen::MatrixXd &Vel,const Eigen::MatrixXd &Acc,const Eigen::VectorXd &Time)
{
    int p_order   = 2 * d_order - 1;
    int p_num1d   = p_order + 1;

    int m = Time.size();
    MatrixXd PolyCoeff = MatrixXd::Zero(m, 2 * p_num1d);
    VectorXd Px(p_num1d * m), Py(p_num1d * m);

    VectorXd *Vector_P[3]={&Px,&Py};
    MatrixXd Matrix_Q=MatrixXd::Zero(p_num1d * m,p_num1d * m);
    MatrixXd Matrix_M=MatrixXd::Zero(p_num1d * m,p_num1d * m);
    MatrixXd Matrix_CT=MatrixXd::Zero(p_num1d * m,d_order*2+(m-1)*d_order);
    MatrixXd Matrix_R=MatrixXd::Zero(p_num1d * m,p_num1d * m);
    VectorXd Vector_d(d_order*2+(m-1)*d_order);
    VectorXd Vector_df(d_order*2+(m-1));
    VectorXd Vector_dp((m-1)*(d_order-1));

    for(int n=0;n<m;n++)
    {
        for(int i=d_order;i<p_num1d;i++)
        {
            for(int j=d_order;j<p_num1d;j++)
            {
                Matrix_Q(n*p_num1d+i,n*p_num1d+j)=(double)Factorial(i)/Factorial(i-d_order)*Factorial(j)/Factorial(j-d_order)/(i+j-p_order)* pow(Time(n),(i+j-p_order));
            }
        }
    }
    for(int n=0;n<m;n++)
    {
        for(int i=0;i<p_num1d;i++)
        {
            if(i<p_num1d/2)
            {
                Matrix_M(n*p_num1d+i,n*p_num1d+i)=Factorial(i);
            }
            else
            {
                int tmp=i-p_num1d/2;
                for(int j=tmp;j<p_num1d;j++)
                {
                    Matrix_M(n*p_num1d+i,n*p_num1d+j)=pow(Time(n),j-tmp)*Factorial(j)/Factorial(j-tmp);
                }
            }
        }
    }
    int CT_row=0,CT_col=0;
    for(int i=0;i<d_order;i++)
    {
        Matrix_CT(CT_row,CT_col)=1;
        CT_row++;
        CT_col++;
    }
    for(int i=0;i<m-1;i++)
    {
        Matrix_CT(CT_row,CT_col)=1;
        CT_row+=d_order;
        Matrix_CT(CT_row,CT_col)=1;
        CT_row+=d_order;
        CT_col++;
    }
    for(int i=0;i<d_order;i++)
    {
        Matrix_CT(CT_row,CT_col)=1;
        CT_row++;
        CT_col++;
    }
    CT_row=d_order+1;
    for(int i=0;i<m-1;i++)
    {
        for(int j=0;j<d_order-1;j++)
        {
            Matrix_CT(CT_row,CT_col)=1;
            Matrix_CT(CT_row+d_order,CT_col)=1;
            CT_row++;
            CT_col++;
        }
        CT_row+=d_order+1;
    }

    Matrix_R=Matrix_CT.transpose()*Matrix_M.inverse().transpose()*Matrix_Q*Matrix_M.inverse()*Matrix_CT;
    MatrixXd R_pp=Matrix_R.bottomRightCorner(Vector_dp.size(),Vector_dp.size());
    MatrixXd R_fp=Matrix_R.topRightCorner(Vector_df.size(),Vector_dp.size());
    
    for(int axis_num=0;axis_num<2;axis_num++)
    {
        Vector_df(0)=Path(0,axis_num);
        Vector_df(d_order+m-1)=Path(m,axis_num);
        Vector_df(1)=Vel(0,axis_num);
        Vector_df(d_order+m)=Vel(1,axis_num);
        Vector_df(2)=Acc(0,axis_num);
        Vector_df(d_order+m+1)=Acc(1,axis_num);
        for(int i=3;i<d_order;i++)
        {
            Vector_df(i)=0;
            Vector_df(d_order+m-1+i)=0;
        }
        for(int i=0;i<m-1;i++)
        {
            Vector_df(d_order+i)=Path(i+1,axis_num);
        }
        Vector_dp=-R_pp.inverse()*R_fp.transpose()*Vector_df;
        VectorXd Vector_d(Vector_df.size()+Vector_dp.size());
        Vector_d.topRows(Vector_df.size())=Vector_df;
        Vector_d.bottomRows(Vector_dp.size())=Vector_dp;
        *Vector_P[axis_num]=Matrix_M.inverse()*Matrix_CT*Vector_d;
    }
    for(int i=0;i<m;i++)
    {
        for(int axis_num=0;axis_num<2;axis_num++)
        {
            PolyCoeff.block(i,axis_num*p_num1d,1,p_num1d)=Vector_P[axis_num]->block(i*p_num1d,0,p_num1d,1).transpose();
        }
    }
    return PolyCoeff;
}